#include "node_main.hpp"

#define OBSTACLE_AVOIDANCE_FILE_NAME "/ObstaclePlan.xml"

using namespace std::chrono_literals;
using namespace std::placeholders;

namespace AgvObstaAvoid
{

AvoidNode::AvoidNode(const rclcpp::NodeOptions & options)
: Node("agv_obstacle_avoidance", options),
  lidar_node_mon_(false),
  avoid_node_mon_(false),
  agv_work_mode_(false),
  charge_(false),
  agv_state_(false),
  lidar_equip_empty_(false),
  io_equip_empty_(false),
  camera_equip_empty_(false),
  io_enable_(true),
  stop_update_(false),
  laser_avoid_(std::make_shared<LaserObstaAvoid>()),             
  // io_avoid_(std::make_shared<IoObstaAvoid>(shared_from_this())), 
  scenes_judge_(std::make_shared<ScenesJudge>()),                
  scan_filter_(std::make_shared<ScanFilterChain>()),             
  // camera_avoid_(std::make_shared<DepthCamera>(shared_from_this())),
  first_callback_(true),
  last_event_time_(std::chrono::steady_clock::now())
{
}

bool AvoidNode::InitAvoidanceParam()
{
    RCLCPP_INFO(this->get_logger(), "start read to InitAvoidanceParam");
    const char *filename = agv_avoid_file_path_.c_str();
    tinyxml2::XMLDocument doc;
    if (doc.LoadFile(filename) != tinyxml2::XML_SUCCESS) {
        RCLCPP_ERROR(this->get_logger(), "ObstaclePlan.xml open failed");
        return false;
    }

    tinyxml2::XMLElement *root = doc.RootElement();
    if (!root) {
        RCLCPP_ERROR(this->get_logger(), "ObstaclePlan.xml root is nullptr");
        return false;
    }
    
    // clear lidar and io equip
    try {
        laser_avoid_->laser_equip_list_.clear();
        io_avoid_->io_equip_list_.clear();
        scenes_judge_->plan_list.clear();
        camera_avoid_->camera_equip_list.clear();
        RCLCPP_INFO(this->get_logger(), "clear lidar and io equip successed");
    } catch (const std::exception &e) {
        RCLCPP_ERROR(this->get_logger(), "variable lidar and io equip list not be cleared");
        return false;
    }

    tinyxml2::XMLElement *first_element = root->FirstChildElement();
    while (first_element) {
        RCLCPP_INFO(this->get_logger(), "start to read: %s", first_element->Name());
        if (std::string(first_element->Name()) == "equiplist") {
            tinyxml2::XMLElement *equip_node = first_element->FirstChildElement();
            if (!equip_node) {
                RCLCPP_ERROR(this->get_logger(), "ObstaclePlan.xml equip_node is nullptr");
                return false;
            }
            while (equip_node) {
                if (std::string(equip_node->Attribute("type")) == "graphic") {
                    bool ret = laser_avoid_->LaserReadXml(equip_node);
                    if (!ret) {
                        RCLCPP_ERROR(this->get_logger(), "lidar avoidance obstacle xml open failed");
                        return false;
                    }
                } else if (std::string(equip_node->Attribute("type")) == "dido") {
                    bool ret = io_avoid_->IOReadXml(equip_node);
                    if (!ret) {
                        RCLCPP_ERROR(this->get_logger(), "io avoidance obstacle xml open failed");
                        return false;
                    }
                } else if (std::string(equip_node->Attribute("type")) == "graphic_3d") {
                    bool ret = camera_avoid_->CameraReadXml(equip_node);
                    if (!ret) {
                        RCLCPP_ERROR(this->get_logger(), "camera avoidance obstacle xml open failed");
                        return false;
                    }
                }
                equip_node = equip_node->NextSiblingElement();
            }
        } else if (std::string(first_element->Name()) == "scenes") {
            bool ret = scenes_judge_->ScenesReadXml(first_element);
            if (!ret) {
                RCLCPP_ERROR(this->get_logger(), "scenes avoidance obstacle xml open failed");
                return false;
            }
        } else if (std::string(first_element->Name()) == "filter") {
            scan_filter_->filter_enable = 
                    first_element->Attribute("enable") ? std::stoi(first_element->Attribute("enable")) : false;

            if (scan_filter_->filter_enable) {
                RCLCPP_INFO(this->get_logger(), "lidar scan filter open");
                bool ret = scan_filter_->FilterReadXml(first_element);
                if (!ret) {
                    RCLCPP_ERROR(this->get_logger(), "filter avoidance obstacle xml open failed");
                    return false;
                }
            } else {
                RCLCPP_INFO(this->get_logger(), "lidar scan filter close");
            }
        }
        RCLCPP_INFO(this->get_logger(), "%s readout completed", first_element->Name());
        first_element = first_element->NextSiblingElement();
    }
    
    RCLCPP_INFO(this->get_logger(), 
                "InitAvoidanceParam lidar equip size:%zu io equip size:%zu scenes size:%zu",
                laser_avoid_->laser_equip_list_.size(),
                io_avoid_->io_equip_list_.size(),
                scenes_judge_->plan_list.size());

    lidar_equip_empty_ = laser_avoid_->laser_equip_list_.empty();
    io_equip_empty_ = io_avoid_->io_equip_list_.empty();
    camera_equip_empty_ = camera_avoid_->camera_equip_list.empty();

    RCLCPP_INFO(this->get_logger(), "lidar_equip_empty: %d io_equip_empty: %d", 
                lidar_equip_empty_, io_equip_empty_);

    if (!io_equip_empty_) {
        if (io_enable_) {
            bool ret = io_avoid_->init();
            if (!ret) return false;
            io_enable_ = false;
        }
        
        io_avoid_->outputMap2IO(scenes_judge_->dido_equip_manual);
    }
    
    return true;
}

void AvoidNode::init()
{
    // read params files
    const char* home_path = getenv("HOME");
    agv_avoid_file_path_ = std::string(home_path) + "/params" + OBSTACLE_AVOIDANCE_FILE_NAME;
    RCLCPP_INFO(this->get_logger(), "agv avoid file path: %s", agv_avoid_file_path_.c_str());

    io_avoid_ = std::make_shared<IoObstaAvoid>(shared_from_this());
    camera_avoid_ = std::make_shared<DepthCamera>(shared_from_this());
    
    avoid_node_mon_ = !InitAvoidanceParam();
    camera_avoid_->init();

    size_t equip_num = laser_avoid_->laser_equip_list_.size();
    switch (equip_num)
    {
    case zero:
        break;

    case one:
        scan_filter_->scan_filter_1_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan_filtered_1", 1);
       
        lidar_sub_1_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            laser_avoid_->laser_equip_list_[0].topicname, rclcpp::SensorDataQoS(),
            [this, name = laser_avoid_->laser_equip_list_[0].name](sensor_msgs::msg::LaserScan::SharedPtr scan) {
                scan_filter_->LidarSubCallback(scan, name);
            });

        laser_sub_1_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan_filtered_1", 1,
            [this, inversion = laser_avoid_->laser_equip_list_[0].inversion, 
                    name = laser_avoid_->laser_equip_list_[0].name](sensor_msgs::msg::LaserScan::SharedPtr scan) {
                laser_avoid_->ScanCallback(scan, inversion, name, this->now());
            });
        break;

    case two:
        scan_filter_->scan_filter_1_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan_filtered_1", 1);
        scan_filter_->scan_filter_2_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan_filtered_2", 1);

        lidar_sub_1_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            laser_avoid_->laser_equip_list_[0].topicname, 1,
            [this, name = laser_avoid_->laser_equip_list_[0].name](sensor_msgs::msg::LaserScan::SharedPtr scan) {
                scan_filter_->LidarSubCallback(scan, name);
            });
        
        lidar_sub_2_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            laser_avoid_->laser_equip_list_[1].topicname, 1,
            [this, name = laser_avoid_->laser_equip_list_[1].name](sensor_msgs::msg::LaserScan::SharedPtr scan) {
                scan_filter_->LidarSubCallback(scan, name);
            });

        laser_sub_1_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan_filtered_1", 1,
            [this, inversion = laser_avoid_->laser_equip_list_[0].inversion, 
                    name = laser_avoid_->laser_equip_list_[0].name](sensor_msgs::msg::LaserScan::SharedPtr scan) {
                laser_avoid_->ScanCallback(scan, inversion, name, this->now());
            });
        
        laser_sub_2_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan_filtered_2", 1,
            [this, inversion = laser_avoid_->laser_equip_list_[1].inversion, 
                    name = laser_avoid_->laser_equip_list_[1].name](sensor_msgs::msg::LaserScan::SharedPtr scan) {
                laser_avoid_->ScanCallback(scan, inversion, name, this->now());
            });
        break;
    
    default:
        break;
    }

    event_pub_ = this->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 1);
    avoid_pub_ = this->create_publisher<agv_msgs::msg::AvoidObstaInfo>("/agv_obstacle_level", 10);
    agv_mode_sub_ = this->create_subscription<agv_msgs::msg::AGVMode>(
        "/agvmode", 2, std::bind(&AvoidNode::AgvModeCallback, this, _1));
    odom_sub_ = this->create_subscription<agv_msgs::msg::OdometryLite>(
        "/odom", 10, std::bind(&AvoidNode::OdomCallback, this, _1));
    auto_status_sub_ = this->create_subscription<agv_msgs::msg::AgvAutoStatus>(
        "/auto_run_status", 10, std::bind(&AvoidNode::AutoStatusCallback, this, _1));
    peripher_sub_ = this->create_subscription<agv_msgs::msg::PeriphStatus>(
        "/periphStatus", 10, std::bind(&AvoidNode::PeripheralCallback, this, _1));

    update_avoid_file_ = this->create_service<agv_srvs::srv::AvoidObstaUpdate>(
        "/agv_avoid_update", 
        std::bind(&AvoidNode::HandleAvoidObstaUpdate, this, _1, _2));
    
    avoid_switch_ser_ = this->create_service<agv_srvs::srv::AvoidObstaSwitch>(
        "/agv_obstacle_avoidance_server", 
        std::bind(&AvoidNode::HandleManualAvoidSwitch, this, _1, _2));
    
    combined_timer_ = this->create_wall_timer(
        20ms,
        [this]() {
            if (first_callback_) {
                last_event_time_ = std::chrono::steady_clock::now();
                first_callback_ = false;
            }

            if (!stop_update_) {
                this->update();
            }

            auto now = std::chrono::steady_clock::now();
            auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
                now - last_event_time_).count();

            if (elapsed >= 1000) {
                this->PublishEvent();
                last_event_time_ = now;  
            }
        });
}

void AvoidNode::PublishEvent()
{
    auto event_msg = std::make_unique<agv_msgs::msg::AGVEvent>();
    static uint32_t seq = 0;
    auto stamptmp = this->now();
    event_msg->header.stamp = stamptmp;
    event_msg->header.frame_id = this->get_name();
    event_msg->ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
    event_msg->type = agv_msgs::msg::AGVEvent::INFO;
    event_msg->action = agv_msgs::msg::AGVEvent::NONE;
    event_msg->param1 = avoid_node_mon_;
    event_msg->param2 = lidar_node_mon_;

    if (avoid_node_mon_ || lidar_node_mon_) {
        if (avoid_node_mon_) {
            std::string event_details = "ObstaclePlan.xml data is error";
            event_msg->description = event_details;
        } else if (lidar_node_mon_) {
            std::string event_details = "avoid lidar data time out";
            event_msg->description = event_details;
        }
        event_msg->eventtime = stamptmp.seconds();
        event_msg->ecode = agv_msgs::msg::AGVEvent::OBSTACLE_AVOID_DATA_ERROR;
        event_msg->action = agv_msgs::msg::AGVEvent::ESTOP;
        event_msg->type = agv_msgs::msg::AGVEvent::ALARM;
        event_msg->ackreq = true;
    }
    event_pub_->publish(std::move(event_msg));
}

void AvoidNode::HandleAvoidObstaUpdate(
    const std::shared_ptr<agv_srvs::srv::AvoidObstaUpdate::Request> req,
    std::shared_ptr<agv_srvs::srv::AvoidObstaUpdate::Response> resp)
{
    RCLCPP_INFO(this->get_logger(), "agv avoid obstacle xml file start update");
    if (req->start == true) {
        // 调用此服务暂停循环update
        stop_update_ = true;
        RCLCPP_INFO(this->get_logger(), "stop main update: %d", stop_update_);
        bool update = InitAvoidanceParam();
        if (!update) {
            RCLCPP_ERROR(this->get_logger(), "agv avoidance obstacle update failed");
            resp->status = false;
        } else {
            RCLCPP_INFO(this->get_logger(), "agv avoidance obstacle update successed");
            resp->status = true;
            agv_state_ = true;
        }
    }
    // 服务结束后恢复循环update
    stop_update_ = false;
    RCLCPP_INFO(this->get_logger(), "recover main update: %d", stop_update_);
}

void AvoidNode::HandleManualAvoidSwitch(
    const std::shared_ptr<agv_srvs::srv::AvoidObstaSwitch::Request> req,
    std::shared_ptr<agv_srvs::srv::AvoidObstaSwitch::Response> resp)
{
    std::string plan_name = req->planname;
    bool ret = scenes_judge_->ScenesSwitch(plan_name);
    bool set = io_avoid_->outputMap2IO(scenes_judge_->dido_equip_manual);
    if (ret && set) {
        resp->status = true;
        RCLCPP_INFO(this->get_logger(), "manual agv avoid obstacle scanes switch successed");
    } else {
        resp->status = false;
        RCLCPP_INFO(this->get_logger(), "manual agv avoid obstacle scanes switch failed");
    }
}

void AvoidNode::AgvModeCallback(const agv_msgs::msg::AGVMode::SharedPtr info_msg) 
{
    int state = static_cast<int>(info_msg->mode);
    if (state == 0) {
        agv_work_mode_ = 0;  // manul
    } else {
        agv_work_mode_ = 1;  // auto
    }
}

void AvoidNode::BatteryCallback(const agv_msgs::msg::BatInfo::SharedPtr bat_msg)
{
    if (bat_msg->current > 0.0)
        charge_ = true;
    else
        charge_ = false;
}

void AvoidNode::OdomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_msg)
{
    laser_avoid_->OdomCallback(odom_msg);
    scenes_judge_->OdomCallback(odom_msg);
}

void AvoidNode::AutoStatusCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr status_msg)
{
    laser_avoid_->PathCallback(status_msg);
    scenes_judge_->PathCallback(status_msg);
    io_avoid_->autoStatusCallBack(status_msg);
}

void AvoidNode::PeripheralCallback(const agv_msgs::msg::PeriphStatus::SharedPtr peripher_msg)
{
    scenes_judge_->LoadCallback(peripher_msg);
    io_avoid_->peripheralCallback(peripher_msg);
}

void AvoidNode::update()
{
    auto msg_danger = std::make_unique<agv_msgs::msg::AvoidObstaInfo>();
    msg_danger->time = this->now();
    msg_danger->scenes_name = scenes_judge_->scenes_name;

    if (laser_avoid_->TimeoutCheck(msg_danger->time) || 
        io_avoid_->FaultDetec() || camera_avoid_->TimeoutCheck(msg_danger->time)) {
        lidar_node_mon_ = true;
    } else {
        lidar_node_mon_ = false;
    }
    
    if (agv_work_mode_) {// 0-manual  1-auto
    
        agv_state_ = scenes_judge_->AgvStatePolling();
        if (agv_state_) {
            avoid_result_ = scenes_judge_->PlanEquipDistribute();
            if (!avoid_result_.dido_equip.empty())
                io_avoid_->outputMap2IO(avoid_result_.dido_equip);
            agv_state_ = false;
        }
       
        if (!lidar_equip_empty_) {
            std::vector<agv_msgs::msg::AvoidObsta> tmp = laser_avoid_->ScanCheck(avoid_result_.scan_equip);
            msg_danger->avoid_msg.insert(msg_danger->avoid_msg.end(), tmp.begin(), tmp.end());
        }

        if (!io_equip_empty_) {
            std::vector<agv_msgs::msg::AvoidObsta> tmp = io_avoid_->ScanCheck(avoid_result_.dido_equip);
            msg_danger->avoid_msg.insert(msg_danger->avoid_msg.end(), tmp.begin(), tmp.end());
        }

        if (!camera_equip_empty_) {
            std::vector<agv_msgs::msg::AvoidObsta> tmp = camera_avoid_->CloudCheck(avoid_result_.came_equip);
            msg_danger->avoid_msg.insert(msg_danger->avoid_msg.end(), tmp.begin(), tmp.end());
        }
    } else {
        scenes_judge_->agv_state_main = {};

        if (!lidar_equip_empty_) {
            std::vector<agv_msgs::msg::AvoidObsta> tmp = laser_avoid_->ScanCheck(scenes_judge_->scan_equip_manual);
            msg_danger->avoid_msg.insert(msg_danger->avoid_msg.end(), tmp.begin(), tmp.end());
        }

        if (!io_equip_empty_) {
            std::vector<agv_msgs::msg::AvoidObsta> tmp = io_avoid_->ScanCheck(scenes_judge_->dido_equip_manual);
            msg_danger->avoid_msg.insert(msg_danger->avoid_msg.end(), tmp.begin(), tmp.end());
        }

        if (!camera_equip_empty_) {
            std::vector<agv_msgs::msg::AvoidObsta> tmp = camera_avoid_->CloudCheck(scenes_judge_->came_equip_manual);
            msg_danger->avoid_msg.insert(msg_danger->avoid_msg.end(), tmp.begin(), tmp.end());
        }
    }

    // 自动空闲状态下或者充电状态下，屏蔽所有避障
    if ((agv_work_mode_ && scenes_judge_->workstate == 0) || charge_) {
        for (auto & msg : msg_danger->avoid_msg) {
            msg.warn = false;
            msg.slow = false;
            msg.stop = false;
        }
    }

    avoid_pub_->publish(std::move(msg_danger));
}

} // namespace AgvObstaAvoid

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<AgvObstaAvoid::AvoidNode>(rclcpp::NodeOptions());
    node->init();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}